/**
 * @file ContBTBuilder.cpp
 * @brief Implementation of the algorithm to build the behavior tree from the plan tree
 * @date 2024-2-27
*/

#include "contbtsys_bt_builder/ContBTBuilder.hpp"

namespace ContBTSys {
    std::string ContBTBuilder::t(int level) {
        std::string ret;
        for (int i = 0; i < level; i++) {
            ret = ret + "  ";
        }
        return ret;
    }
        
    std::string ContBTBuilder::buildBT(ActionPtr action_node, int level) {
        std::string ret = "";
        const std::string action_id = action_node->get_name();

        // Actuation Action
        if (action_node->get_type() == ActionType::ACTUATION) {
            if (action_node->son != nullptr) {
                ret += t(level) + "<Sequence name=\"" + action_id + "\">\n";
                ret += t(level + 1) + "<Action ID=\"" + action_id + "\" name=\"" + action_id + "\"/>\n";
                ret += buildBT(action_node->son, level + 1);
                ret += t(level) + "</Sequence>\n";
            } else {
                ret += t(level) + "<Action ID=\"" + action_id + "\" name=\"" + action_id + "\"/>\n";
            }
        } else { // Observation Action
            if (action_node->true_son != nullptr && action_node->false_son != nullptr) {
                ret += t(level) + "<Fallback name=\"" + action_id + "\">\n";
                ret += t(level + 1) + "<Sequence name=\"" + action_id + "_TRUESON\">\n";
                ret += t(level + 2) + "<Action ID=\"" + action_id + "\" name=\"" + action_id + "\"/>\n";
                ret += buildBT(action_node->true_son, level + 2);
                ret += t(level + 1) + "</Sequence>\n";
                ret += buildBT(action_node->false_son, level + 1);
                ret += t(level) + "</Fallback>\n";
            } else if (action_node->true_son != nullptr && action_node->false_son == nullptr) {
                ret += t(level) + "<Sequence name=\"" + action_id + "\">\n";
                ret += t(level + 1) + "<Action ID=\"" + action_id + "\" name=\"" + action_id + "\"/>\n";
                ret += buildBT(action_node->true_son, level + 1);
                ret += t(level) + "</Sequence>\n";
            } else if (action_node->true_son == nullptr && action_node->false_son != nullptr) {
                ret += t(level) + "<Fallback name=\"" + action_id + "\">\n";
                ret += t(level + 1) + "<Action ID=\"" + action_id + "\" name=\"" + action_id + "\"/>\n";
                ret += buildBT(action_node->false_son, level + 1);
                ret += t(level) + "</Fallback>\n";
            } 
        }

        return ret;
    }

    std::string ContBTBuilder::getBT(
        const std::string & domain, const std::string & problem,
        const std::string & node_namespace
    ) {
        // get the plan tree
        RootPtr root = cont_planner_ptr_->getPlan(domain, problem, node_namespace);

        // build the behavior tree
        std::string bt_plan = "";
        bt_plan = std::string("<root main_tree_to_execute=\"MainTree\">\n") +
                    t(1) + "<BehaviorTree ID=\"MainTree\">\n";

        if (root != nullptr) bt_plan = bt_plan + buildBT(root, 2);

        bt_plan = bt_plan + t(1) + "</BehaviorTree>\n</root>\n";

        return bt_plan;
    }
}   // namespace ContBTSys